#ifndef CELLDECOMPASTAR_H
#define CELLDECOMPASTAR_H

/********** Library Includes **************************************************/

#include <opencv/cv.h>

/********** Classes ***********************************************************/

class CellDecompAStar {
public:
	explicit CellDecompAStar(const cv::Mat &grid);
	
	std::vector<cv::Point> plan(cv::Point start, cv::Point goal);
	
private:
	// TODO These should to be moved out
	
	struct Node {
		explicit Node() : valid(false), visited(false) {
			neighbors.reserve(4);
		}
		
		explicit Node(cv::Point location) :
			location(location),
			valid(true),
			visited(false)
		{
			neighbors.reserve(4);
		}
		
		cv::Point location;
		std::vector<Node *> neighbors;
		bool valid;
		bool visited;
	};
	
	struct Path {
		explicit Path(Node *node, int cost, int expected) :
			cost(cost),
			expected(expected)
		{
			nodes.push_back(node);
		}
		
		explicit Path(const Path &path, Node *node, int additional, int expected) :
			cost(path.cost + additional),
			expected(expected)
		{
			nodes.insert(nodes.begin(), path.nodes.begin(), path.nodes.end());
			nodes.push_back(node);
		}
		
		bool operator<(const Path &path) const {
			return cost + expected > path.cost + path.expected;
		}
		
		std::vector<Node *> nodes;
		
		/**
		 * The actual cost to travel along this path from the start node.
		 */
		int cost;
		
		/**
		 * The expected cost (heuristic) to travel from the end of this path to
		 * the goal.
		 */
		int expected;
	};
	
	int manhatten(cv::Point one, cv::Point two);
	
	bool isValid(const cv::Point &) const;
	
	/**
	 * Converts a vector of Node pointers representing the path to travel to a
	 * vector of points to drive.
	 *
	 * The point vector is "compressed" such that only points requiring a turn
	 * are representing.  For example, if the Node pointer vector contains Nodes
	 * at (3, 3), (4, 3), (5, 3), and (5, 4), the resulting Point vector will
	 * contain (3, 3), (5, 3), and (5, 4).
	 */
	std::vector<cv::Point> toPoints(const std::vector<Node *> &path) const;
	
	/**
	 * Returns the unit vector in the direction (two - one).
	 */
	std::vector<double> unit(cv::Point one, cv::Point two) const;
	
	typedef std::vector<Node> NodeVector;
	typedef std::vector<NodeVector> NodeMatrix;
	NodeMatrix m_nodes;
};

#endif